# coding=utf-8
# 2019/12/9: 首个记录
# 2019/12/13: 修正类构造函数
# 2020/4/1: 导入bi_common
# 2020/4/9: 支持v4；车辆参数赋初值
# 2020/7/28: 完善枚举类型定义
# 2020/8/3: 增加枚举类型==/!=重载
# 2020/8/5: 枚举值类改为继承EnumValue
# 2023/5/5: 修正turn赋值

import bi_common as bi


# 档位状态
class GearPositionValue(bi.EnumValue):
    def __str__(self):
        if self.val == 1:
            return 'PARK'
        elif self.val == 2:
            return 'REVERSE'
        elif self.val == 3:
            return 'NEUTRAL'
        elif self.val == 4:
            return 'DRIVE'
        else:
            return 'UNKNOWN'


class GearPosition:
    UNKNOWN = GearPositionValue(0)  # 未知
    PARK = GearPositionValue(1)  # P档
    REVERSE = GearPositionValue(2)  # R档
    NEUTRAL = GearPositionValue(3)  # N档
    DRIVE = GearPositionValue(4)  # D档

    def __init__(self):
        pass


# 转向灯状态
class TurnStateValue(bi.EnumValue):
    def __str__(self):
        if self.val == 1:
            return 'NONE'
        elif self.val == 2:
            return 'TURN_LEFT'
        elif self.val == 3:
            return 'TURN_RIGHT'
        elif self.val == 4:
            return 'EMERGENCY'
        else:
            return 'UNKNOWN'


class TurnState:
    UNKNOWN = TurnStateValue(0)  # 未知
    NONE = TurnStateValue(1)  # 未打灯
    TURN_LEFT = TurnStateValue(2)  # 左转向灯
    TURN_RIGHT = TurnStateValue(3)  # 右转向灯
    EMERGENCY = TurnStateValue(4)  # 双闪

    def __init__(self):
        pass


# 喇叭状态
class HornValue(bi.EnumValue):
    def __str__(self):
        if self.val == 1:
            return 'NONE'
        elif self.val == 2:
            return 'ACTIVE'
        else:
            return 'UNKNOWN'


class Horn:
    UNKNOWN = HornValue(0)  # 未知
    NONE = HornValue(1)  # 不响
    ACTIVE = HornValue(2)  # 响

    def __init__(self):
        pass


# 大灯状态
class HeadLightValue(bi.EnumValue):
    def __str__(self):
        if self.val == 1:
            return 'NONE'
        elif self.val == 2:
            return 'NEAR'
        elif self.val == 3:
            return 'FAR'
        else:
            return 'UNKNOWN'


class HeadLight:
    UNKNOWN = HeadLightValue(0)  # 未知
    NONE = HeadLightValue(1)  # 未激活
    NEAR = HeadLightValue(2)  # 近光
    FAR = HeadLightValue(3)  # 远光

    def __init__(self):
        pass


# 雨刮状态
class WiperValue(bi.EnumValue):
    def __str__(self):
        if self.val == 1:
            return 'NONE'
        elif self.val == 2:
            return 'SINGLE'
        elif self.val == 3:
            return 'SLOW1'
        elif self.val == 4:
            return 'SLOW2'
        elif self.val == 5:
            return 'SLOW3'
        elif self.val == 6:
            return 'SLOW4'
        elif self.val == 7:
            return 'SLOW5'
        elif self.val == 8:
            return 'MEDIUM'
        elif self.val == 9:
            return 'FAST'
        else:
            return 'UNKNOWN'


class Wiper:
    UNKNOWN = WiperValue(0)  # 未知
    NONE = WiperValue(1)  # 未激活
    SINGLE = WiperValue(2)  # 单次
    SLOW1 = WiperValue(3)  # 慢 1
    SLOW2 = WiperValue(4)  # 慢 2
    SLOW3 = WiperValue(5)  # 慢 3
    SLOW4 = WiperValue(6)  # 慢 4
    SLOW5 = WiperValue(7)  # 慢 5
    MEDIUM = WiperValue(8)  # 中档
    FAST = WiperValue(9)  # 快档

    def __init__(self):
        pass


# 本车信息样本
class VehicleSample:
    def __init__(self):
        self.time = 0.0  # 时间戳 s
        self.speed = 0.0  # 本车车速 kph
        self.yaw_rate_valid = False
        self.yaw_rate = 0.0  # 横摆角速度 deg/s
        self.curvature_valid = False
        self.curvature = 0.0  # 行驶曲率 1/m 左转为正 未来路径: y=(curvature/2)*(x^2)+(curvature)*(frontOverhang+wheelBase)*x
        self.kilometer_age = 0.0  # 总里程数 km
        self.accel_x_valid = False
        self.accel_x = 0.0  # 本车纵向加速度 m/s2
        self.accel_y_valid = False
        self.accel_y = 0.0  # 本车横向加速度 m/s2
        self.jerk_x_valid = False
        self.jerk_x = 0.0  # 本车纵向急动度 m/s3
        self.jerk_y_valid = False
        self.jerk_y = 0.0  # 本车横向急动度 m/s3
        self.vehicle_width = 1.9  # 车宽 m
        self.vehicle_length = 4.6  # 车长 m
        self.vehicle_height = 1.5  # 车高 m
        self.steer_angle_ratio = 15  # 传动比
        self.wheel_base = 2.8  # 轴距 m
        self.rear_tread = 1.6  # 轮距 m
        self.front_overhang = 0.9  # 前悬 m
        self.steer_angle_valid = False
        self.steer_angle = 0.0  # 方向盘转角 deg
        self.steer_angle_rate_valid = False
        self.steer_angle_rate = 0.0  # 方向盘转角速度 deg/s
        self.steer_torque_valid = False
        self.steer_torque = 0.0  # 方向盘力矩 Nm
        self.brake_valid = False
        self.brake = 0.0  # 刹车位置 %
        self.throttle_valid = False
        self.throttle = 0.0  # 油门位置 %
        self.gear = GearPosition.UNKNOWN  # 档位
        self.turn = TurnState.UNKNOWN  # 转向灯
        self.horn = Horn.UNKNOWN  # 喇叭
        self.head_light = HeadLight.UNKNOWN  # 大灯
        self.wiper = Wiper.UNKNOWN  # 雨刮
        self.fl_speed_valid = False
        self.fl_speed = 0.0  # 左前轮轮速 kph
        self.fr_speed_valid = False
        self.fr_speed = 0.0  # 右前轮轮速 kph
        self.rl_speed_valid = False
        self.rl_speed = 0.0  # 左后轮轮速 kph
        self.rr_speed_valid = False
        self.rr_speed = 0.0  # 右后轮轮速 kph
        self.engine_speed_valid = False
        self.engine_speed = 0.0  # 引擎转速 rpm
        self.engine_torque_valid = False
        self.engine_torque = 0.0  # 引擎扭矩 Nm
        self.front_end_lateral_speed_valid = False
        self.front_end_lateral_speed = 0.0  # 前保中心横向速度 m/s
        self.rear_end_lateral_speed_valid = False
        self.rear_end_lateral_speed = 0.0  # 后保中心横向速度 m/s

    # 转通用样本，用于样本输出
    def to_general_sample(self):
        output = bi.agency.create_general_sample()
        output.protocol = "vehicle-sample-v4"
        output.time = self.time
        output.significant = 40
        output.values = []
        output.values.append(0)
        output.values.append(0)
        output.values.append(self.speed)
        output.values.append(self.yaw_rate if self.yaw_rate_valid else None)
        output.values.append(self.curvature if self.curvature_valid else None)
        output.values.append(self.accel_x if self.accel_x_valid else None)
        output.values.append(self.accel_y if self.accel_y_valid else None)
        output.values.append(self.vehicle_width)
        output.values.append(self.vehicle_length)
        output.values.append(self.vehicle_height)
        output.values.append(self.steer_angle_ratio)
        output.values.append(self.wheel_base)
        output.values.append(self.rear_tread)
        output.values.append(self.front_overhang)
        output.values.append(self.steer_angle if self.steer_angle_valid else None)
        output.values.append(self.steer_angle_rate if self.steer_angle_rate_valid else None)
        output.values.append(self.brake if self.brake_valid else None)
        output.values.append(self.throttle if self.throttle_valid else None)
        output.values.append(self.gear.val)
        output.values.append(self.turn.val)
        output.values.append(self.fl_speed if self.fl_speed_valid else None)
        output.values.append(self.fr_speed if self.fr_speed_valid else None)
        output.values.append(self.rl_speed if self.rl_speed_valid else None)
        output.values.append(self.rr_speed if self.rr_speed_valid else None)
        output.values.append(self.engine_speed if self.engine_speed_valid else None)
        output.values.append(self.engine_torque if self.engine_torque_valid else None)
        output.values.append(self.horn.val)
        output.values.append(self.head_light.val)
        output.values.append(self.wiper.val)
        output.values.append(self.kilometer_age)
        output.values.append(self.front_end_lateral_speed if self.front_end_lateral_speed_valid else None)
        output.values.append(self.rear_end_lateral_speed if self.rear_end_lateral_speed_valid else None)
        output.values.append(self.steer_torque if self.steer_torque_valid else None)
        output.values.append(self.jerk_x if self.jerk_x_valid else None)
        output.values.append(self.jerk_y if self.jerk_y_valid else None)
        output.values.append(None)  # reserved
        output.values.append(None)  # reserved
        output.values.append(None)  # reserved
        output.values.append(None)  # reserved
        output.values.append(None)  # reserved
        return output


def _conv_vehicle_sample_v3(gs):
    values_count = len(gs.values)
    if values_count < 27:
        return None
    trajectory_size = int(gs.values[0]) if gs.values[0] is not None else 0
    if values_count != 27 and values_count != 27 + 2 * trajectory_size:
        return None
    output = VehicleSample()
    output.time = gs.time
    output.speed = float(gs.values[1]) if gs.values[1] is not None else 0.0
    if gs.values[2] is not None:
        output.yaw_rate_valid = True
        output.yaw_rate = float(gs.values[2])
    if gs.values[3] is not None:
        output.curvature_valid = True
        output.curvature = float(gs.values[3])
    if gs.values[4] is not None:
        output.accel_x_valid = True
        output.accel_x = float(gs.values[4])
    if gs.values[5] is not None:
        output.accel_y_valid = True
        output.accel_y = float(gs.values[5])
    output.vehicle_width = float(gs.values[6]) if gs.values[6] is not None else 1.9
    output.vehicle_length = float(gs.values[7]) if gs.values[7] is not None else 4.6
    output.steer_angle_ratio = float(gs.values[8]) if gs.values[8] is not None else 15.0
    output.wheel_base = float(gs.values[9]) if gs.values[9] is not None else 2.8
    output.rear_tread = float(gs.values[10]) if gs.values[10] is not None else 1.6
    output.front_overhang = float(gs.values[11]) if gs.values[11] is not None else 0.9
    if gs.values[12] is not None:
        output.steer_angle_valid = True
        output.steer_angle = float(gs.values[12])
    if gs.values[13] is not None:
        output.brake_valid = True
        output.brake = float(gs.values[13])
    if gs.values[14] is not None:
        output.throttle_valid = True
        output.throttle = float(gs.values[14])
    output.gear = GearPositionValue(int(gs.values[15])) if gs.values[15] is not None else GearPosition.UNKNOWN
    output.turn = TurnStateValue(int(gs.values[16])) if gs.values[16] is not None else TurnState.UNKNOWN
    if gs.values[17] is not None:
        output.fl_speed_valid = True
        output.fl_speed = float(gs.values[17])
    if gs.values[18] is not None:
        output.fr_speed_valid = True
        output.fr_speed = float(gs.values[18])
    if gs.values[19] is not None:
        output.rl_speed_valid = True
        output.rl_speed = float(gs.values[19])
    if gs.values[20] is not None:
        output.rr_speed_valid = True
        output.rr_speed = float(gs.values[20])
    if gs.values[21] is not None:
        output.engine_speed_valid = True
        output.engine_speed = float(gs.values[21])
    if gs.values[22] is not None:
        output.engine_torque_valid = True
        output.engine_torque = float(gs.values[22])
    output.horn = HornValue(int(gs.values[23])) if gs.values[23] is not None else Horn.UNKNOWN
    output.head_light = HeadLightValue(int(gs.values[24])) if gs.values[24] is not None else HeadLight.UNKNOWN
    output.wiper = WiperValue(int(gs.values[25])) if gs.values[25] is not None else Wiper.UNKNOWN
    output.kilometer_age = float(gs.values[26]) if gs.values[26] is not None else 0.0
    return output


def _conv_vehicle_sample_v4(gs):
    values_count = len(gs.values)
    if values_count < 40:
        return None
    contour_size = int(gs.values[0]) if gs.values[0] is not None else 0
    trajectory_size = int(gs.values[1]) if gs.values[1] is not None else 0
    if values_count != 40 + 2 * contour_size and values_count != 40 + 2 * (contour_size + trajectory_size):
        return None
    output = VehicleSample()
    output.time = gs.time
    output.speed = float(gs.values[2]) if gs.values[2] is not None else 0.0
    if gs.values[3] is not None:
        output.yaw_rate_valid = True
        output.yaw_rate = float(gs.values[3])
    if gs.values[4] is not None:
        output.curvature_valid = True
        output.curvature = float(gs.values[4])
    if gs.values[5] is not None:
        output.accel_x_valid = True
        output.accel_x = float(gs.values[5])
    if gs.values[6] is not None:
        output.accel_y_valid = True
        output.accel_y = float(gs.values[6])
    output.vehicle_width = float(gs.values[7]) if gs.values[7] is not None else 1.9
    output.vehicle_length = float(gs.values[8]) if gs.values[8] is not None else 4.6
    output.vehicle_height = float(gs.values[9]) if gs.values[9] is not None else 1.5
    output.steer_angle_ratio = float(gs.values[10]) if gs.values[10] is not None else 15.0
    output.wheel_base = float(gs.values[11]) if gs.values[11] is not None else 2.8
    output.rear_tread = float(gs.values[12]) if gs.values[12] is not None else 1.6
    output.front_overhang = float(gs.values[13]) if gs.values[13] is not None else 0.9
    if gs.values[14] is not None:
        output.steer_angle_valid = True
        output.steer_angle = float(gs.values[14])
    if gs.values[15] is not None:
        output.steer_angle_rate_valid = True
        output.steer_angle_rate = float(gs.values[15])
    if gs.values[16] is not None:
        output.brake_valid = True
        output.brake = float(gs.values[16])
    if gs.values[17] is not None:
        output.throttle_valid = True
        output.throttle = float(gs.values[17])
    output.gear = GearPositionValue(int(gs.values[18])) if gs.values[18] is not None else GearPosition.UNKNOWN
    output.turn = TurnStateValue(int(gs.values[19])) if gs.values[19] is not None else TurnState.UNKNOWN
    if gs.values[20] is not None:
        output.fl_speed_valid = True
        output.fl_speed = float(gs.values[20])
    if gs.values[21] is not None:
        output.fr_speed_valid = True
        output.fr_speed = float(gs.values[21])
    if gs.values[22] is not None:
        output.rl_speed_valid = True
        output.rl_speed = float(gs.values[22])
    if gs.values[23] is not None:
        output.rr_speed_valid = True
        output.rr_speed = float(gs.values[23])
    if gs.values[24] is not None:
        output.engine_speed_valid = True
        output.engine_speed = float(gs.values[24])
    if gs.values[25] is not None:
        output.engine_torque_valid = True
        output.engine_torque = float(gs.values[25])
    output.horn = HornValue(int(gs.values[26])) if gs.values[26] is not None else Horn.UNKNOWN
    output.head_light = HeadLightValue(int(gs.values[27])) if gs.values[27] is not None else HeadLight.UNKNOWN
    output.wiper = WiperValue(int(gs.values[28])) if gs.values[28] is not None else Wiper.UNKNOWN
    output.kilometer_age = float(gs.values[29]) if gs.values[29] is not None else 0.0
    if gs.values[30] is not None:
        output.front_end_lateral_speed_valid = True
        output.front_end_lateral_speed = float(gs.values[30])
    if gs.values[31] is not None:
        output.rear_end_lateral_speed_valid = True
        output.rear_end_lateral_speed = float(gs.values[31])
    if gs.values[32] is not None:
        output.front_end_lateral_speed_valid = True
        output.front_end_lateral_speed = float(gs.values[32])
    if gs.values[33] is not None:
        output.jerk_x_valid = True
        output.jerk_x = float(gs.values[33])
    if gs.values[34] is not None:
        output.jerk_y_valid = True
        output.jerk_y = float(gs.values[34])
    return output


def _interpolate_vehicle_sample(vs1, w1, vs2, w2):
    output = VehicleSample()
    output.time = bi.time
    output.speed = vs1.speed * w1 + vs2.speed * w2
    output.kilometer_age = vs1.kilometer_age * w1 + vs2.kilometer_age * w2
    if vs1.yaw_rate_valid and vs2.yaw_rate_valid:
        output.yaw_rate_valid = True
        output.yaw_rate = vs1.yaw_rate * w1 + vs2.yaw_rate * w2
    if vs1.curvature_valid and vs2.curvature_valid:
        output.curvature_valid = True
        output.curvature = vs1.curvature * w1 + vs2.curvature * w2
    if vs1.accel_x_valid and vs2.accel_x_valid:
        output.accel_x_valid = True
        output.accel_x = vs1.accel_x * w1 + vs2.accel_x * w2
    if vs1.accel_y_valid and vs2.accel_y_valid:
        output.accel_y_valid = True
        output.accel_y = vs1.accel_y * w1 + vs2.accel_y * w2
    if vs1.jerk_x_valid and vs2.jerk_x_valid:
        output.jerk_x_valid = True
        output.jerk_x = vs1.jerk_x * w1 + vs2.jerk_x * w2
    if vs1.jerk_y_valid and vs2.jerk_y_valid:
        output.jerk_y_valid = True
        output.jerk_y = vs1.jerk_y * w1 + vs2.jerk_y * w2
    output.vehicle_width = vs1.vehicle_width
    output.vehicle_length = vs1.vehicle_length
    output.vehicle_height = vs1.vehicle_height
    output.steer_angle_ratio = vs1.steer_angle_ratio
    output.wheel_base = vs1.wheel_base
    output.rear_tread = vs1.rear_tread
    output.front_overhang = vs1.front_overhang
    if vs1.steer_angle_valid and vs2.steer_angle_valid:
        output.steer_angle_valid = True
        output.steer_angle = vs1.steer_angle * w1 + vs2.steer_angle * w2
    if vs1.steer_angle_rate_valid and vs2.steer_angle_rate_valid:
        output.steer_angle_rate_valid = True
        output.steer_angle_rate = vs1.steer_angle_rate * w1 + vs2.steer_angle_rate * w2
    if vs1.steer_torque_valid and vs2.steer_torque_valid:
        output.steer_torque_valid = True
        output.steer_torque = vs1.steer_torque * w1 + vs2.steer_torque * w2
    if vs1.brake_valid and vs2.brake_valid:
        output.brake_valid = True
        output.brake = vs1.brake * w1 + vs2.brake * w2
    if vs1.throttle_valid and vs2.throttle_valid:
        output.throttle_valid = True
        output.throttle = vs1.throttle * w1 + vs2.throttle * w2
    output.gear = vs1.gear if w1 > w2 else vs2.gear
    output.turn = vs1.turn if w1 > w2 else vs2.turn
    output.horn = vs1.horn if w1 > w2 else vs2.horn
    output.head_light = vs1.head_light if w1 > w2 else vs2.head_light
    output.wiper = vs1.wiper if w1 > w2 else vs2.wiper
    if vs1.fl_speed_valid and vs2.fl_speed_valid:
        output.fl_speed_valid = True
        output.fl_speed = vs1.fl_speed * w1 + vs2.fl_speed * w2
    if vs1.fr_speed_valid and vs2.fr_speed_valid:
        output.fr_speed_valid = True
        output.fr_speed = vs1.fr_speed * w1 + vs2.fr_speed * w2
    if vs1.rl_speed_valid and vs2.rl_speed_valid:
        output.rl_speed_valid = True
        output.rl_speed = vs1.rl_speed * w1 + vs2.rl_speed * w2
    if vs1.rr_speed_valid and vs2.rr_speed_valid:
        output.rr_speed_valid = True
        output.rr_speed = vs1.rr_speed * w1 + vs2.rr_speed * w2
    if vs1.engine_speed_valid and vs2.engine_speed_valid:
        output.engine_speed_valid = True
        output.engine_speed = vs1.engine_speed * w1 + vs2.engine_speed * w2
    if vs1.engine_torque_valid and vs2.engine_torque_valid:
        output.engine_torque_valid = True
        output.engine_torque = vs1.engine_torque * w1 + vs2.engine_torque * w2
    if vs1.front_end_lateral_speed_valid and vs2.front_end_lateral_speed_valid:
        output.front_end_lateral_speed_valid = True
        output.front_end_lateral_speed = vs1.front_end_lateral_speed * w1 + vs2.front_end_lateral_speed * w2
    if vs1.rear_end_lateral_speed_valid and vs2.rear_end_lateral_speed_valid:
        output.rear_end_lateral_speed_valid = True
        output.rear_end_lateral_speed = vs1.rear_end_lateral_speed * w1 + vs2.rear_end_lateral_speed * w2
    return output


# 获取VehicleSample，用于样本输入
def get_vehicle_sample():
    vs1 = None
    vs2 = None
    w1 = 0.0
    w2 = 0.0
    protocol_id_v3 = 'vehicle-sample-v3'
    protocol_id_v4 = 'vehicle-sample-v4'
    if protocol_id_v4 in bi.input_samples:
        pair = bi.input_samples[protocol_id_v4]
        vs1 = _conv_vehicle_sample_v4(pair.sample1)
        w1 = pair.weight1
        vs2 = _conv_vehicle_sample_v4(pair.sample2)
        w2 = pair.weight2
    elif protocol_id_v3 in bi.input_samples:
        pair = bi.input_samples[protocol_id_v3]
        vs1 = _conv_vehicle_sample_v3(pair.sample1)
        w1 = pair.weight1
        vs2 = _conv_vehicle_sample_v3(pair.sample2)
        w2 = pair.weight2
    if vs1 is not None and vs2 is not None:
        return _interpolate_vehicle_sample(vs1, w1, vs2, w2)
    return None
